
#ifndef avp_slam_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_
#define avp_slam_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_

#include <pcl/registration/icp.h>
#include "registration_interface.hpp"
#include <omp.h>

namespace avp_slam {
    class ICPRegistration : public RegistrationInterface {
    public:

        ICPRegistration(const YAML::Node &node);

        ICPRegistration(float max_dis, float trans_eps, int max_iter, float euclidean_eps);

        bool SetInputTarget(const CloudData::CLOUD_PTR &input_target) override;

        Eigen::Matrix4f ScanMatch(const CloudData::CLOUD_PTR &input_source,
                                  const Eigen::Matrix4f &predict_pose,
                                  CloudData::CLOUD_PTR &result_cloud_ptr) override;

        float GetFitnessScore() override;

        bool GetHasConverged() override;

    private:
        bool SetRegistrationParam(float max_dis, float trans_eps, int max_iter, float euclidean_eps);

    private:
        pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>::Ptr icp_ptr_;

        float sumTime;

        uint32_t cnt;

        bool _hasConverged = false;

        float _fitnessScore = 10.f;

        float GetAverTime();
    };
}

#endif